﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Sim3D_practicum.Components;

namespace Sim3D_practicum.Physics
{
    class Plain : ICollidableObject
    {
        private float A { get; set; }
        private float B { get; set; }
        private float C { get; set; }
        private float D { get; set; }

        private float HeightSquared { get; set; }
        private float WidthSquared  { get; set; }
        private float Height { get; set; }
        private float Width { get; set; }

        //the corner of the border.
        public Vector3 Position { get; set; }

        //the normal vector that is perpendicular both to the position and the normal of the position
        public Vector3 NormalVector { get; private set; }

        //the directions to which the collisionplain goes
        public Vector3 Direction1 { get; private set; }
        public Vector3 Direction2 { get; private set; }
        public Rectangle drawRect;

        /// <summary>
        /// creates a new plain
        /// </summary>
        /// <param name="dir1">should not be normalized, for te length will be used as height</param>
        /// <param name="dir2">should not be normalized, for te length will be used as width</param>
        /// <param name="position">the poosition of the corner of this plain from the origin.</param>
        public Plain( Vector3 dir1, Vector3 dir2, Vector3 position )
        {
            //Set the position of this Vector
            Position = position;
            //as well as the directions in which it is going to stretch
            Direction1 = dir1;
            Direction2 = dir2;

            Width = dir2.Length();
            Height = dir1.Length();

            WidthSquared = dir2.LengthSquared( );
            HeightSquared = dir1.LengthSquared( );

            Direction1 = Vector3.Normalize( dir1 );
            Direction2 = Vector3.Normalize( dir2 );

            //and finally, we need the NormalVector in order to know which direction to look for collision from the ball
            NormalVector = Vector3.Normalize( Vector3.Cross( Direction1, Direction2 ) );

            SetEquasion();
            float p1x, p2x, p1z, p2z;
            if (true)//position.X < Direction1.X * Height + Direction2.X * Width)
            {
                p1x = position.X;
                p2x = Direction1.X * Height + Direction2.X * Width;
            }
            else
            {
                p1x = Direction1.X * Height + Direction2.X * Width;
                p2x = position.X;
            }
            if (true)//position.Z < Direction1.Z * Height + Direction2.Z * Width)
            {
                p1z = position.Z;
                p2z = Direction1.Z * Height + Direction2.Z * Width;
            }
            else
            {
                p1z = Direction1.Z * Height + Direction2.Z * Width;
                p2z = position.Z;
            }
            drawRect = new Rectangle(Calculator.mmToPixel(p1x), Calculator.mmToPixel(p1z),
                Calculator.mmToPixel(p2x),
                Calculator.mmToPixel(p2z));
        }
        /// <summary>
        /// set this borders plain equasion, so we can check whether or not an object will have interaction with it.
        /// </summary>
        private void SetEquasion( )
        {
            A = NormalVector.X;
            B = NormalVector.Y;
            C = NormalVector.Z;
            D = A * Position.X +
                B * Position.Y +
                C * Position.Z;
        }
        public bool CheckForCollision( IBallCollision ball, GameTime gt, out Vector3 r )
        {
            r = new Vector3( 0, 0, 0 );
            Vector3 ray;
            float dot = Vector3.Dot( ball.Velocity, NormalVector );
            //if the dotproduct is less than 0, the angle is greater than 90 degrees, and the ball is not moving towards the border, and we can
            //save a lot of calculations
            if ( ( dot < 0 ) )
                goto ReturnFalse;
            //System.Diagnostics.Debug.WriteLine("past Dot");
            //we are trying to cast a ray from the ball upon the plain.
            if ( !CanCastRay( ball.Position, out ray ) )
                //if this cannot be done ( i.e. the ball is not in the plain ), then we do nothing
                goto ReturnFalse;
            ray = Vector3.Subtract( ray, ball.Position );
            float rayLength = ray.Length( );
            //otherwise, we use the ray to check where the collision will most likely take place. The ray is the position
            //from the position of the ball. If the length is less than or equal to the distance of the sphere + its radius, we have 
            //a collision
            //if ( rayLength > Ball.RADIUS )
            //    goto ReturnFalse;

            //if the length of the ray is smaller than the radius of the ball ( our ray is shot from the center of the ball, so the side of the sphere
            //should be position.length + radius            
            if ( rayLength - ( ball.Velocity.Length( ) * ( ( float ) gt.ElapsedGameTime.Milliseconds / 1000 ) ) - Ball.RADIUS <= 0 )
            {
                r = -2 * dot * NormalVector + ball.Velocity;
                return true;
            }
            ReturnFalse:
            return false;                              
        }
        public void CheckForCollision( IBallCollision ball, GameTime gt )
        {
            Vector3 r;
            if ( CheckForCollision( ball, gt, out r ) )
            {
                ball.CollisionDetected( r );
            }
        }
        /// <summary>
        /// We cast a ray from the position q to the normalvector. Ray = q + time * n.
        /// checks if the object is inside the boundaries of the plain. If so, returns true otherwise returns false
        /// </summary>
        /// <param name="q">the position from where to cast the ray</param>
        /// <returns>returns the ray as a vector3</returns>
        private bool CanCastRay( Vector3 q, out Vector3 ray )
        {
            //get the time in length it will take before the ray will hit the border
            float time = GetTime( q );
            ray = Vector3.Add( q, Vector3.Multiply( NormalVector, time ) );
            return IsInsideBoundaries( ray );
        }
        /// <summary>
        /// Gets the time it takes in length from position q to the normalvector of the plain
        /// </summary>
        /// <param name="q">the position from which to shoot the ray</param>
        /// <returns>returns the time in length it takes a vector to get from point q to the normalvector of the plain</returns>
        private float GetTime( Vector3 q )
        {
            float x = q.X * A;
            float y = q.Y * B;
            float z = q.Z * C;
            //ax + by + cz = d.
            //ax = A( q.X * ( A * time ) )
            //by = B( q.Y * ( B * time ) )
            //cz = C( q.Z * ( C * time ) ) 
            //A, B and C are already gotten from the normalvector of the plain.
            //this results in: v + ( w * time ) = d. We then take v to the other side: d - v = w * time.
            //and finally we divide both values by w. ( w * time ) / w = time. time = ( d - v ) / w.
            float time = A * A + B * B + C * C;
                        
            return ( D - ( x + y + z ) ) / time;//(1-p.x)/1 of (-1+p.x)/1 of(1-p.z)...
        }
        /// <summary>
        /// checks whether or not the ray hits the plain.
        /// </summary>
        /// <param name="q">q is the position where the ray would hit the plain, if it lies on the plain</param>
        /// <returns>returns true if the ray collides with the plain, otherwise returns false.</returns>
        private bool IsInsideBoundaries( Vector3 q )
        {
            //we are going to check if the x, y and z of the given point have the same lambda and mu to construct, if so, then the point is
            //on the infinite plain p.
            float lambda = 0.0f;
            float mu = 0.0f;
            bool foundL = false;
            bool foundM = false;

            float[] start = new float[3];
            float[] directionL = new float[3];
            float[] directionM = new float[3];
            float[] ray = new float[3];

            start[0] = Position.X;
            start[1] = Position.Y;
            start[2] = Position.Z;

            directionL[0] = Direction1.X;
            directionL[1] = Direction1.Y;
            directionL[2] = Direction1.Z;

            directionM[0] = Direction2.X;
            directionM[1] = Direction2.Y;
            directionM[2] = Direction2.Z;

            ray[0] = q.X;
            ray[1] = q.Y;
            ray[2] = q.Z;
            //the equasion is: q = a * lambda + b * mu, where q is the point to check, a is the horizontal vector and b is the vertical vector.
            //first we find out what lambda is. To do so, we need to make the X and Y equasions equal in such a way that we can leave mu out 
            //of the picture. We do this by multiplying the entire Y equasion by a factor , so that the muX and muY become equally big. This way
            //we have a lambdaX - startpointX = lambdaY - startPointY.
            float factor = 1.0f;
            int index = 0;
            while ( true )
            {
                if ( foundL || foundM )
                {
                    if ( foundL )
                    {
                        if ( directionM[index] != 0 )
                        {
                            mu = ( - ( directionL[index] * lambda ) - start[index] + ray[index] ) / directionM[index];
                            break;
                        }
                    } else
                    {
                        if ( directionL[index] != 0 )
                        {
                            lambda = ( -( directionM[index] * mu ) - start[index] + ray[index] ) / directionL[index];
                            break;
                        }
                    }
                    index++;
                } else
                {
                    if ( directionL[index] == 0 && directionM[index] == 0 )
                    {
                        index++;
                    } else
                    {
                        if ( directionM[index] == 0 )
                        {
                            lambda = ( ray[index] - start[index] ) / directionL[index];
                            foundL = true;
                        } else if ( directionL[index] == 0 )
                        {
                            mu = ( ray[index] - start[index] ) / directionM[index];
                            foundM = true;
                        } else
                        {
                            int nextIndex = ( index + 1 ) % 3;
                            factor = directionM[index] / directionM[nextIndex];
                            lambda = ( ( start[nextIndex] * factor ) - start[index] - ( ray[nextIndex] * factor ) + ray[index] ) /
                                     ( directionL[index] - ( directionL[nextIndex] * factor ) );
                            foundL = true;
                        }
                        index = 0;
                    }
                }
            }
            
            //we then take startPointX and take it to the other side of the equals sign ( 3 + 2 = 5 is the same as 3 = 5 - 2. Same thing with variables )
            //then we take the lambdaY to the other side of the equals sign, our equasion is now: lambdaX - ( lambdaY * factor ) = pointX - ( pointY * factor )
            //all we have to do now, is divide both sides by the difference of lambdaX - ( lambdaY * factor ). Because, consider this example:
            //in our equasion, lambdaX = 1, ( lambdaY * factor ) = -2, PointX + ( pointY * factor ) = -12. What we do is: ( lambdaX - lambdaY * factor ) = 3.
            //we already took lambdaY to the other side, so it becomes positive. so our answer, -12, should be divided by 3, and our final answer is -4

            //now that we have our lambda, it is relatively easy to gain our mu, we just fill it into the first equasion, and then extract mu

            //check if the given lambda and mu give us the same answers with all three equasions, if they do not, then the point does not lie on
            //our infinite plain.
            float x = start[0] + directionL[0] * lambda + directionM[0] * mu;
            float y = start[1] + directionL[1] * lambda + directionM[1] * mu;
            float z = start[2] + directionL[2] * lambda + directionM[2] * mu;
            if ( !( Math.Abs( x - q.X ) <= 0.01f && Math.Abs( y - q.Y ) <= 0.01f && Math.Abs( z - q.Z ) <= 0.01f ) )
                //so, we return false
                return false;
            //but if they do have the same answers, we still have to do one final test in order to check if the ray collides with our FINITE plain,
            //but this is rather easy.
            return true;
            return Vector3.Multiply( Direction1, lambda ).LengthSquared( ) <= WidthSquared &&
                   Vector3.Multiply( Direction2, mu ).LengthSquared( ) <= HeightSquared;
        }

        public Rectangle drawFace()
        {
            return drawRect;
        }
    }
}
